import numpy as np


def avoid_collisions(current_positions, current_directions, target_positions, max_speed):
    num_robots = 4
    new_directions = current_directions

    # 计算每个机器人的下一位置
    next_positions = current_positions + new_directions * max_speed / 50

    # 计算机器人之间的距离矩阵
    distances = np.sqrt(np.sum((next_positions[:, np.newaxis, :] - next_positions) ** 2, axis=2))

    # 检查每个机器人是否与其他机器人发生碰撞
    for i in range(num_robots):
        for j in range(i + 1, num_robots):
            if distances[i, j] < 2:
                # 如果发生碰撞，将机器人方向更改为不与任何其他机器人碰撞的方向
                direction = target_positions[i] - current_positions[i]
                angle = np.arctan2(direction[1], direction[0])
                while True:
                    angle += np.pi / 4
                    new_direction = np.array([np.cos(angle), np.sin(angle)])
                    new_next_position = current_positions[i] + new_direction * max_speed
                    if np.sqrt(np.sum((new_next_position - current_positions[j]) ** 2)) > 2:
                        new_directions[i] = new_direction
                        break

    return new_directions


if __name__ == '__main__':
    current_positions = np.array([[1., 1.], [1., 2.], [6, 7], [41.38605499, 48.25009155]])
    current_directions = np.array([[0., 6.], [0., -6.], [0.2442743182, 2.587517977], [-4.739554882, 2.064558983]])
    target_positions = np.array([[4.75, 48.75], [14.75, 48.75], [34.75, 48.75], [39.75, 48.75]])
    radius = [0.45, 0.45, 0.45, 0.45]

    # 使用 avoid_collisions 函数计算每个机器人的方向
    max_speed = 6
    next_directions = avoid_collisions(current_positions, current_directions, target_positions, max_speed)

    # 计算每个机器人的下一位置
    next_positions = current_positions + current_directions * max_speed

    print(current_positions)
    print(next_positions)
    print(current_directions)
    print(next_directions)